#include "rclcpp/rclcpp.hpp"
#include "rms_pkg/msg/heartbeat.hpp"
#include "../include/global_logger.h"
#include <unordered_map>
#include <mutex>
#include <memory>

class HeartbeatListener : public rclcpp::Node {
public:
    HeartbeatListener() : Node("heartbeat_listener") {

        LOG_INFO(" HeartbeatListener 初始化开始");

        this->declare_parameter<int>("timeout", 5);

        int timeout = 5;
        this->get_parameter("timeout", timeout);
        if (timeout <= 0) {
            LOG_WARN(" 无效的超时设置，使用默认值 5s");
            timeout = 5;
        }

        auto qos = rclcpp::QoS(10).reliable();
        subscription_ = this->create_subscription<rms_pkg::msg::Heartbeat>(
            "heartbeat", qos,
            [this, timeout](const rms_pkg::msg::Heartbeat::SharedPtr msg) {
                this->heartbeat_callback(msg, timeout);
            });

        timer_ = this->create_wall_timer(
            std::chrono::seconds(timeout / 2 + 1),
            [this, timeout]() {
                this->check_timeout(timeout);
            });

        LOG_INFO(" 创建心跳订阅者，超时时间: {}s", timeout);
        LOG_INFO(" 创建定时器，检查间隔: {}s", timeout / 2 + 1);
        LOG_INFO(" HeartbeatListener 初始化完成");
    }

private:
    void heartbeat_callback(const rms_pkg::msg::Heartbeat::SharedPtr msg, int timeout) {
        std::lock_guard<std::mutex> lock(map_mutex_);
        std::string key = msg->robot_type + "/" + msg->robot_id;
        last_heartbeat_[key] = msg->timestamp;
        LOG_DEBUG(" 收到心跳包: {}/{} 超时时间:{}s", msg->robot_type, msg->robot_id, timeout);
    }

    void check_timeout(int timeout) {
        uint64_t now = static_cast<uint64_t>(
            std::chrono::duration_cast<std::chrono::microseconds>(
                std::chrono::system_clock::now().time_since_epoch()
            ).count()
        );

        std::lock_guard<std::mutex> lock(map_mutex_);
        for (auto it = last_heartbeat_.begin(); it != last_heartbeat_.end();) {
            if ((now - it->second) > static_cast<uint64_t>(timeout * 1e6)) {
                LOG_WARN(" 客户端 {} 超时!", it->first);
                it = last_heartbeat_.erase(it);
            } else {
                ++it;
            }
        }
    }

    rclcpp::Subscription<rms_pkg::msg::Heartbeat>::SharedPtr subscription_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::unordered_map<std::string, uint64_t> last_heartbeat_;
    std::mutex map_mutex_;
};

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);

    // 初始化全局日志记录器（如果尚未初始化）
    GlobalLogger::getInstance().initialize("data/logs", "RMSClient_heartbeat", 15);

    rclcpp::spin(std::make_shared<HeartbeatListener>());
    rclcpp::shutdown();
    return 0;
}
